#include "OBB.h"

#include <Iw2D.h>
#include "il_assert.h"
#include <math.h>

void OBB::SetUpCentered( CIwFVec2 pos, CIwFVec2 size, CIwFVec2 orientation )
{
	IL_ASSERT( orientation.GetLengthSquared() != 0, "passed in |0| orientation vector" );

	// we do the calculations of the corner positions around the 0/0
	// origin and roate them afterwards into the correct position
	
	m_size = size;
	m_corners[0] = CIwFVec2( -(size.x/2), -(size.y/2) ); // top left
	m_corners[1] = CIwFVec2( (size.x/2), -(size.y/2) ); // top right
	m_corners[2] = CIwFVec2( (size.x/2), (size.y/2) ); // bottom right
	m_corners[3] = CIwFVec2( -(size.x/2), (size.y/2) ); // bottom left
	
	
	// calculate the rotation
	float rotation =  2.0f * PI - atan2f(orientation.x, orientation.y);
	
	CIwFMat2D transform;
	
	transform.SetRot( rotation, true );
	
	m_corners[0] = pos + transform.RotateVec( m_corners[0] );
	m_corners[1] = pos + transform.RotateVec( m_corners[1] );
	m_corners[2] = pos + transform.RotateVec( m_corners[2] );
	m_corners[3] = pos + transform.RotateVec( m_corners[3] );
	
	// safe the position
	m_pos = pos;
	m_radius_square = MAX( size.x, size.y );
	m_radius_square = m_radius_square;
	m_radius_square = m_radius_square * m_radius_square ;
}

void OBB::UpdateTransform( CIwFVec2 pos, CIwFVec2 orientation )
{
	IL_ASSERT( orientation.GetLengthSquared() != 0, "passed in |0| orientation vector" );

	// we do the calculations of the corner positions around the 0/0
	// origin and roate them afterwards into the correct position
	
	m_corners[0] = CIwFVec2( -(m_size.x/2), -(m_size.y/2) ); // top left
	m_corners[1] = CIwFVec2( (m_size.x/2), -(m_size.y/2) ); // top right
	m_corners[2] = CIwFVec2( (m_size.x/2), (m_size.y/2) ); // bottom right
	m_corners[3] = CIwFVec2( -(m_size.x/2), (m_size.y/2) ); // bottom left
	
	
	// calculate the rotation
	float rotation =  2.0f * PI - atan2f(orientation.x, orientation.y);
	
	CIwFMat2D transform;
	
	transform.SetRot( rotation, true );
	
	m_corners[0] = pos + transform.RotateVec( m_corners[0] );
	m_corners[1] = pos + transform.RotateVec( m_corners[1] );
	m_corners[2] = pos + transform.RotateVec( m_corners[2] );
	m_corners[3] = pos + transform.RotateVec( m_corners[3] );
	
	// safe the position
	m_pos = pos;
}

#ifdef _DEBUG
void OBB::DrawDebug()
{
	Iw2DSetColour( 0xFF000000 );
	Iw2DDrawLine( 
		CIwSVec2((int)m_corners[0].x, (int)m_corners[0].y),
		CIwSVec2((int)m_corners[1].x, (int)m_corners[1].y) );
		
		
	Iw2DDrawLine( 
		CIwSVec2((int)m_corners[1].x, (int)m_corners[1].y),
		CIwSVec2((int)m_corners[2].x, (int)m_corners[2].y) );
		
		
	Iw2DDrawLine( 
		CIwSVec2((int)m_corners[2].x, (int)m_corners[2].y),
		CIwSVec2((int)m_corners[3].x, (int)m_corners[3].y) );
		
	Iw2DDrawLine( 
		CIwSVec2((int)m_corners[3].x, (int)m_corners[3].y),
		CIwSVec2((int)m_corners[0].x, (int)m_corners[0].y) );
	Iw2DSetColour( 0xFFDDFFFF );
}
#endif

bool OBB::CheckLeftScreenBorderIntersection() const
{
	for( int i = 0; i < 4; ++i ) {
		if( m_corners[i].x <= 0 ) {
			return true;
		}
	}
	
	return false;
}

bool OBB::CheckRightScreenBorderIntersection() const
{
	for( int i = 0; i < 4; ++i ) {
		if( m_corners[i].x >= 480 ) {
			return true;
		}
	}
	
	return false;
}

bool OBB::CheckTopScreenBorderIntersection() const
{
	for( int i = 0; i < 4; ++i ) {
		if( m_corners[i].y <= 0 ) {
			return true;
		}
	}
	
	return false;
}

bool OBB::CheckBottomScreenBorderIntersection() const
{
	for( int i = 0; i < 4; ++i ) {
		if( m_corners[i].y >= 320 ) {
			return true;
		}
	}
	
	return false;
}


bool OBB::CheckForOverlapOBB( const OBB &o1, const OBB &o2 )
{
	// firstly do a cheap conservative circle vs circle check
	// in most cases only this test needs to be done
	CIwFVec2 diff = o2.m_pos - o1.m_pos;
	
	float length_square = (float) fabs(diff.GetLengthSquared());
	
	if( length_square > (o1.m_radius_square) + (o2.m_radius_square) ) {
		return false;
	}

	iwfixed t1 = 0;
	iwfixed t2 = 0;
	CIwVec2 collpt;

	// just check evry edge with every egde for intersections
	for( int i = 0; i < 4; ++i ) {
		for( int j = 0; j < 4; ++j ) {
			if( IwIntersectLineLine2D(
				CIwVec2(o1.m_corners[i]),
				CIwVec2(o1.m_corners[(i+1)%4] - o1.m_corners[i]),
				CIwVec2(o2.m_corners[j]),
				CIwVec2(o2.m_corners[(j+1)%4] - o2.m_corners[j]),
				collpt,
				t1,
				t2 ) )
				
				{
					if( t1 <= IW_FIXED_FROM_FLOAT(1.0f) && t1 >= IW_FIXED_FROM_FLOAT(0.0f) && t2 <= IW_FIXED_FROM_FLOAT (1.0f) && t2 >= IW_FIXED_FROM_FLOAT(0.0f) ) {
						return true;
					}
				}
		}
	}
	
	return false;
}

bool OBB::CheckForOverlapLines( const OBB &ship_obb, const CIwFVec2* points, int num_points , CIwFVec2 *out_normal )
{
	iwfixed t1 = 0;
	iwfixed t2 = 0;
	CIwVec2 collpt;

	// just check evry edge with every egde for intersections
	for( int i = 0; i < 4; ++i ) {
		
		for( int p = 0; p < num_points-1; ++p ) {
		
			CIwVec2 pos1 = CIwVec2( ship_obb.m_corners[i] );
			CIwVec2 dir1 = CIwVec2( ship_obb.m_corners[(i+1)%4] - ship_obb.m_corners[i] );
			
			CIwVec2 pos2 = CIwVec2( points[p] );
			CIwVec2 dir2 = CIwVec2( points[p+1] - points[p] );
		
			if( IwIntersectLineLine2D(
				pos1,
				dir1,
				pos2,
				dir2,
				collpt,
				t1,
				t2 ) )
				
				{
					if( t1 <= IW_FIXED_FROM_FLOAT(1.0f) && t1 >= IW_FIXED_FROM_FLOAT(0.0f) && t2 <= IW_FIXED_FROM_FLOAT (1.0f) && t2 >= IW_FIXED_FROM_FLOAT(0.0f) ) {
						
						CIwFVec2 dir = points[p+1] - points[p];
						
						// found a collision, calculate the normal
						CIwFVec2 normal( -dir.y, dir.x );
						normal.Normalise();
						
						*out_normal = normal; 
						
						return true;
						
					}
				}
		}
	}

	return false;
}